/**
 * @Author         : Liuxc
 * @Date           : 2022-11-25 22:54
 * @LastEditTime   : 2025-01-24 16:30
 * @Copyright(c)   : Liuxc
 * @Note : 对于突发读取工况 地址部分 BMI088使用的16bit数据位宽，所以需要读取两个字节
 * 才能开始得到完整的数据。
 *  @Note ACC返回的数据格式
 *  byte:       | 1 | 2 | 3 | 4 |...|
 *  MOSI:       |add|   |   |   |...|
 *  MISO:       |   |add|x1 |x2 |...|
 *   所以需要多读一个字节
 *
 *  Gyro返回的数据格式
 *  byte:       | 1 | 2 | 3 | 4 |...|
 *  MOSI:       |add|   |   |   |...|
 *  MISO:       |   |y1 |y2 |   |...|
 *  从从第二个字节开始就是数据内容了
 * */

#include "bmi088.h"
#include "bmi088reg.h"

#include "gpio.h"
#include "spi.h"
#include "main.h"

#include <math.h>
#include "string.h"



/*姿态传感器读取数据结构体*/
bmi088_data_t BMI088_Struct;

/* 驱动接入函数部分 */
/**
 * @brief 向加速度计写入字节
 * @param addr
 * @param data
 */
void WriteDataToAcc(uint8_t addr, uint8_t data)
{
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
    uint8_t pTxData[2];
    pTxData[0] = (addr & BMI088_SPI_WRITE_CODE); // 高位置0
    pTxData[1] = data;

    HAL_SPI_Transmit(&BMI088_SPI, pTxData, 2, 1000);
    while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_TX)
        ;
    HAL_Delay(1); // 需要延时1ms 保证数据写入正常
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
}

/**
 * @brief 向陀螺仪写入字节
 * @param addr
 * @param data
 */
void WriteDataToGyro(uint8_t addr, uint8_t data)
{
    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_RESET);
    uint8_t pTxData[2];
    pTxData[0] = (addr & BMI088_SPI_WRITE_CODE);
    pTxData[1] = data;
    HAL_SPI_Transmit(&BMI088_SPI, pTxData, 2, 1000);
    while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_TX)
        ;
    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_SET);
}

/**
 * @brief 读取加速度计一个字节
 * @param addr 读取地址
 * @param data 存放数据的空间地址
 */
void ReadSingleDataFromAcc(uint8_t addr, uint8_t *data)
{
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
    uint8_t pTxData[3] = {0xff};
    memset(pTxData, 0, 3);
    pTxData[0] = (addr | BMI088_SPI_READ_CODE);
    uint8_t pRxData[3];
    /* 发送一个字节 返回一个空字节 再返回一个真正的数据字节 */
    HAL_SPI_TransmitReceive(&BMI088_SPI, pTxData, pRxData, 3, 1000);
    while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_TX_RX)
        ;
    *data = pRxData[2];

    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
}

/**
 * @brief 读取陀螺仪的一个字节
 * @param addr 读取的地址
 * @param data 存放数据的地址
 */
void ReadSingleDataFromGyro(uint8_t addr, uint8_t *data)
{
    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_RESET);
    uint8_t pTxData[2] = {0}; // 发送缓冲区
    pTxData[0] = (addr | BMI088_SPI_READ_CODE);
    uint8_t pRxData[2] = {0}; // 接收缓冲区
    HAL_SPI_TransmitReceive(&BMI088_SPI, pTxData, pRxData, 2, 1000);
    while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_TX_RX)
        ;
    *data = pRxData[1];
    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_SET);
}

/**
 * @brief 连续读取加速度计的数据
 * @param addr 读取地址
 * @param len 读取长度
 * @param data 存放数据的地址
 */
void ReadMultiDataFromAcc(uint8_t addr, uint8_t len, uint8_t *data)
{
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
    uint8_t pTxData[len + 2];
    memset(pTxData, 0, len + 2); // 清除发送缓冲区
    pTxData[0] = (addr | BMI088_SPI_READ_CODE);
    uint8_t pRxData[len + 2];
    memset(pRxData, 0, len + 2); // 清除接收缓冲区

    HAL_SPI_TransmitReceive(&BMI088_SPI, pTxData, pRxData, len + 2, 1000);
    while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_TX_RX)
        ;
    memcpy(data, pRxData + 2, len);
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
}

/**
 * @brief 连续读取陀螺仪数据
 * @param addr 读取地址
 * @param len 数据长度
 * @param data 存放数据的地址
 */
void ReadMultiDataFromGyro(uint8_t addr, uint8_t len, uint8_t *data)
{
    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_RESET);
    uint8_t pTxData[len + 1];
    memset(pTxData, 0, len + 1);
    pTxData[0] = (addr | BMI088_SPI_READ_CODE);
    uint8_t pRxData[len + 1];
    memset(pRxData, 0, len + 1);

    HAL_SPI_TransmitReceive(&BMI088_SPI, pTxData, pRxData, len + 1, 1000);
    while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_TX_RX)
        ;
    memcpy(data, pRxData + 1, len);

    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_SET);
}

/**
 * @brief 加速度计ID校验
 * @param  void 空
 * @return 校验结果
 */
bmi088_error_e VerifyAccChipID(void)
{
    uint8_t chip_id;
    ReadSingleDataFromAcc(ACC_CHIP_ID_ADDR, &chip_id);
    if (chip_id != ACC_CHIP_ID_VAL)
    {
        return ACC_CHIP_ID_ERR;
    }
    return NO_ERROR;
}

/**
 * @brief 陀螺仪ID校验
 * @param  void 空
 * @return 校验结果
 */
bmi088_error_e VerifyGyroChipID(void)
{
    uint8_t chip_id;
    ReadSingleDataFromGyro(GYRO_CHIP_ID_ADDR, &chip_id);
    if (chip_id != GYRO_CHIP_ID_VAL)
    {
        return GYRO_CHIP_ID_ERR;
    }
    return NO_ERROR;
}

/**
 * @brief 加速度计自检
 * @param  void 无
 * @return 自检结果
 */
bmi088_error_e VerifyAccSelfTest(void)
{
    // 打开加速度计电源
    WriteDataToAcc(ACC_PWR_CTRL_ADDR, ACC_PWR_CTRL_ON);
    // 加速度计变成正常模式
    WriteDataToAcc(ACC_PWR_CONF_ADDR, ACC_PWR_CONF_ACT);

    acc_raw_data_t pos_data, neg_data;
    WriteDataToAcc(ACC_RANGE_ADDR, ACC_RANGE_24G);                                                       // 设置加速度计测量范围为24g
    WriteDataToAcc(ACC_CONF_ADDR,  ACC_CONF_BWP_NORM << 4 | ACC_CONF_ODR_50_Hz); // 0xA7 设置滤波
    HAL_Delay(10);
    WriteDataToAcc(ACC_SELF_TEST_ADDR, ACC_SELF_TEST_POS); // 设置加速度计自检为正方向
    HAL_Delay(100);
    ReadAccData(&pos_data);                                // 读取正方向加速度计数据
    WriteDataToAcc(ACC_SELF_TEST_ADDR, ACC_SELF_TEST_NEG); // 设置加速度计自检为负方向
    HAL_Delay(100);
    ReadAccData(&neg_data);                                // 读取负方向加速度计数据
    WriteDataToAcc(ACC_SELF_TEST_ADDR, ACC_SELF_TEST_OFF); // 关闭加速度计自检
    HAL_Delay(100);
    if ((fabs(pos_data.x - neg_data.x) > 0.1f) || (fabs(pos_data.y - neg_data.y) > 0.1f) || (fabs(pos_data.z - neg_data.z) > 0.1f)) // 判断加速度计自检结果是否合格
    {
        return ACC_DATA_ERR; // 自检数据不一致
    }
    return NO_ERROR;
}

/**
 * @brief 陀螺仪自检
 * @param  void 无
 * @return 自检结果
 */
bmi088_error_e VerifyGyroSelfTest(void)
{
    WriteDataToGyro(GYRO_SELF_TEST_ADDR, GYRO_SELF_TEST_ON); // 完成自检之后会自动关闭自检
    uint8_t bist_rdy = 0x00, bist_fail;
    while (bist_rdy == 0)
    {
        ReadSingleDataFromGyro(GYRO_SELF_TEST_ADDR, &bist_rdy);
        bist_rdy = (bist_rdy & 0x02) >> 1; // 只看第2位，判断自检是否完成
    }
    ReadSingleDataFromGyro(GYRO_SELF_TEST_ADDR, &bist_fail);
    bist_fail = (bist_fail & 0x04) >> 2; // 只看第3位 判断传感器数据正常
    if (bist_fail == 0)
    {
        return NO_ERROR;
    }
    else
    {
        return GYRO_DATA_ERR;
    }
}

/**
 * @brief BMI硬件初始化校验
 * @param
 * @return 执行状态
 */
bmi088_error_e BMI088_INIT(void)
{
    bmi088_error_e error = NO_ERROR;

    error |= VerifyAccChipID();
    error |= VerifyGyroChipID();
    error |= VerifyAccSelfTest();
    error |= VerifyGyroSelfTest();

    if (error != NO_ERROR)
    {
        return error;
    }

    bmi088_config_struct_t imu_config_init_struct;
    imu_config_init_struct.acc_bwp = ACC_CONF_BWP_NORM;
    imu_config_init_struct.acc_odr = ACC_CONF_ODR_1600_Hz;
    imu_config_init_struct.acc_range = ACC_RANGE_3G;
    imu_config_init_struct.gyro_range = GYRO_RANGE_1000_DEG_S;
    imu_config_init_struct.gyro_bandwidth = GYRO_ODR_2000Hz_BANDWIDTH_230Hz;

    BMI088_CONF_INIT(&imu_config_init_struct);

    bmi088_acc_fifo_config_struct_t acc_fifo_config_init_struct;

    acc_fifo_config_init_struct.fifo_downs = 5;
    acc_fifo_config_init_struct.fifo_wtm = 0;
    acc_fifo_config_init_struct.fifo_mode = 0;
    acc_fifo_config_init_struct.fifo_accen = 1;
    acc_fifo_config_init_struct.fifo_int1_en = 0;
    acc_fifo_config_init_struct.fifo_int2_en = 0;
    acc_fifo_config_init_struct.int1_out = 0;
    acc_fifo_config_init_struct.int1_od = 0;
    acc_fifo_config_init_struct.int1_lvl = 0;
    acc_fifo_config_init_struct.int1_in = 0;
    acc_fifo_config_init_struct.int2_out = 0;
    acc_fifo_config_init_struct.int2_od = 0;
    acc_fifo_config_init_struct.int2_lvl = 0;
    acc_fifo_config_init_struct.int2_in = 0;
    acc_fifo_config_init_struct.int2_drdy = 0;
    acc_fifo_config_init_struct.int2_fwm = 0;
    acc_fifo_config_init_struct.int2_fful = 0;
    acc_fifo_config_init_struct.int1_drdy = 0;
    acc_fifo_config_init_struct.int1_fwm = 0;
    acc_fifo_config_init_struct.int1_full = 0;

    BMI088_ACC_FIFO_INT(&acc_fifo_config_init_struct);

    return error;
}

/**
 * @brief bmi088结构体配置初始化
 * @param
 */
void BMI088_CONF_INIT(bmi088_config_struct_t *arg)
{
    // 加速度计初始化
    // 先软重启，清空所有寄存器
    WriteDataToAcc(ACC_SOFTRESET_ADDR, ACC_SOFTRESET_VAL);
    HAL_Delay(50);
    // 打开加速度计电源
    WriteDataToAcc(ACC_PWR_CTRL_ADDR, ACC_PWR_CTRL_ON);
    // 加速度计变成正常模式
    WriteDataToAcc(ACC_PWR_CONF_ADDR, ACC_PWR_CONF_ACT);

    // 陀螺仪初始化
    // 先软重启，清空所有寄存器
    WriteDataToGyro(GYRO_SOFTRESET_ADDR, GYRO_SOFTRESET_VAL);
    HAL_Delay(50);
    // 陀螺仪变成正常模式
    WriteDataToGyro(GYRO_LPM1_ADDR, GYRO_LPM1_NOR);

    // 加速度计配置写入
    uint8_t CONFIG =  (arg->acc_bwp << 4) | (arg->acc_odr);
    WriteDataToAcc(ACC_CONF_ADDR, CONFIG);
    // 写入范围
    WriteDataToAcc(ACC_RANGE_ADDR, arg->acc_range);

    // 陀螺仪配置写入
    WriteDataToGyro(GYRO_RANGE_ADDR, arg->gyro_range);
    // 写入带宽
    WriteDataToGyro(GYRO_BANDWIDTH_ADDR, arg->gyro_bandwidth);
}
/**
 * @brief 加速度计FIFO功能配置以及中断等高级功能的配置
 * @param arg 配置参数的结构体指针
 */
void BMI088_ACC_FIFO_INT(bmi088_acc_fifo_config_struct_t *arg)
{
    WriteDataToAcc(ACC_FIFO_DOWNS_ADDR, ACC_FIFO_DOWNS_VAL(arg->fifo_downs));
    WriteDataToAcc(ACC_FIFO_WTM_0_ADDR, (arg->fifo_wtm & 0xff));      // 低八位
    WriteDataToAcc(ACC_FIFO_WTM_1_ADDR, (arg->fifo_wtm >> 8) & 0x1f); // 高五位并且保证其值不会超过63
    WriteDataToAcc(ACC_FIFO_CONFIG_0_ADDR, ACC_FIFO_CONFIG_0_FIFO_MODE(arg->fifo_mode));
    WriteDataToAcc(ACC_FIFO_CONFIG_1_ADDR, ACC_FIFO_CONFIG_1_FIFO_SOURCES(arg->fifo_accen, arg->fifo_int1_en, arg->fifo_int2_en));
    WriteDataToAcc(ACC_INT1_IO_CTRL_ADDR, ACC_INT1_IO_CONF_VAL(arg->int1_in, arg->int1_out, arg->int1_od, arg->int1_lvl));
    WriteDataToAcc(ACC_INT2_IO_CTRL_ADDR, ACC_INT2_IO_CONF_VAL(arg->int2_in, arg->int2_out, arg->int2_od, arg->int2_lvl));
    WriteDataToAcc(ACC_INT_MAP_DATA_ADDR, ACC_INT1_MAP_DATA_VAL(arg->int2_drdy, arg->int2_fwm, arg->int2_fful, arg->int1_drdy, arg->int1_fwm, arg->int1_full));
}
/***/
void BMI088_GYRO_FIFO_INT(bmi088_gyro_fifo_config_struct_t *arg)
{
}
/**
 * @brief 读取加速度数据
 * @param data 加速度计原始数据地址 acc_raw_data_t *
 */
void ReadAccData(acc_raw_data_t *data)
{
    uint8_t buf[ACC_XYZ_LEN], range;
    int16_t acc[3];
    ReadSingleDataFromAcc(ACC_RANGE_ADDR, &range);
    ReadMultiDataFromAcc(ACC_X_LSB_ADDR, ACC_XYZ_LEN, buf);
    acc[0] = ((int16_t)buf[1] << 8) + (int16_t)buf[0];
    acc[1] = ((int16_t)buf[3] << 8) + (int16_t)buf[2];
    acc[2] = ((int16_t)buf[5] << 8) + (int16_t)buf[4];
    data->x = (float)acc[0] / 32768.0f * 3 * powf(2, range);
    data->y = (float)acc[1] / 32768.0f * 3 * powf(2, range);
    data->z = (float)acc[2] / 32768.0f * 3 * powf(2, range);
}

/**
 * @brief 读取陀螺仪数据
 * @param data 传出读取值的数据指针
 */
void ReadGyroData(gyro_raw_data_t *data)
{
    uint8_t buf[GYRO_XYZ_LEN], range;
    int16_t gyro[3];
    float unit;
    ReadSingleDataFromGyro(GYRO_RANGE_ADDR, &range);
    switch (range) // 传感器阈值 选择合适的换算比例
    {
    case 0x00:
        unit = 16.384;
        break;
    case 0x01:
        unit = 32.768;
        break;
    case 0x02:
        unit = 65.536;
        break;
    case 0x03:
        unit = 131.072;
        break;
    case 0x04:
        unit = 262.144;
        break;
    default:
        unit = 16.384;
        break;
    }
    ReadMultiDataFromGyro(GYRO_RATE_X_LSB_ADDR, GYRO_XYZ_LEN, buf);
    gyro[0] = ((int16_t)buf[1] << 8) + (int16_t)buf[0];
    gyro[1] = ((int16_t)buf[3] << 8) + (int16_t)buf[2];
    gyro[2] = ((int16_t)buf[5] << 8) + (int16_t)buf[4];
    data->roll = (float)gyro[0] / unit * DEG2SEC;
    data->pitch = (float)gyro[1] / unit * DEG2SEC;
    data->yaw = (float)gyro[2] / unit * DEG2SEC;
}

/**
 * @brief 读取加速计时间
 * @param time 反馈读取值参数指针
 * @return 时间差
 */
void ReadAccSensorTime(float *time)
{
    uint8_t buf[SENSORTIME_LEN];
    ReadMultiDataFromAcc(SENSORTIME_0_ADDR, SENSORTIME_LEN, buf);
    *time = buf[0] * SENSORTIME_0_UNIT + buf[1] * SENSORTIME_1_UNIT + buf[2] * SENSORTIME_2_UNIT;
}
/**
 * @brief 读取加速度数据和时间
 * @param arg 传出读取值的数据结构体指针
 */
void ReadAccDataAndTime(acc_data_t *arg)
{
    uint8_t buf[ACC_XYZ_LEN + SENSORTIME_LEN], range;
    int16_t acc[3];
    ReadSingleDataFromAcc(ACC_RANGE_ADDR, &range);
    ReadMultiDataFromAcc(ACC_X_LSB_ADDR, ACC_XYZ_LEN + SENSORTIME_LEN, buf);
    acc[0] = ((int16_t)buf[1] << 8) + (int16_t)buf[0];
    acc[1] = ((int16_t)buf[3] << 8) + (int16_t)buf[2];
    acc[2] = ((int16_t)buf[5] << 8) + (int16_t)buf[4];
    arg->acc_raw_data.x = (float)acc[0] / 32768.0f * 3 * powf(2, range);
    arg->acc_raw_data.y = (float)acc[1] / 32768.0f * 3 * powf(2, range);
    arg->acc_raw_data.z = (float)acc[2] / 32768.0f * 3 * powf(2, range);

    static float LAST_TEMP_UNIT = 0.0f;
    LAST_TEMP_UNIT = arg->sensor_time; // get last time
    arg->sensor_time = buf[6] * SENSORTIME_0_UNIT + buf[7] * SENSORTIME_1_UNIT + buf[8] * SENSORTIME_2_UNIT;
    arg->sensor_diff_time = arg->sensor_time - LAST_TEMP_UNIT;
    if (arg->sensor_diff_time < 0)
    {
        arg->sensor_diff_time = arg->sensor_time + 655.35 - LAST_TEMP_UNIT;
    }
}
/**
 * @brief 读取加速度计温度
 * @param temp
 */
void ReadAccTemperature(float *temp)
{
    uint8_t buf[TEMP_LEN];
    ReadMultiDataFromAcc(TEMP_MSB_ADDR, TEMP_LEN, buf);
    uint16_t temp_uint11 = (buf[0] << 3) + (buf[1] >> 5);
    int16_t temp_int11;
    if (temp_uint11 > 1023)
    {
        temp_int11 = (int16_t)temp_uint11 - 2048;
    }
    else
    {
        temp_int11 = (int16_t)temp_uint11;
    }
    *temp = (float)temp_int11 * TEMP_UNIT + TEMP_BIAS;
}

void ReadACC_FIFO_Data(acc_data_t *data)
{
    uint8_t head = 0;

    // 片选加速度计 在CS电平未被拉高之前 本次通信的地址都会默认增加 不用担心地址会混乱
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);

    uint8_t pTxData[3];
    memset(pTxData, 0, 3);

    uint8_t pRxData[3];
    memset(pRxData, 0, 3);

    pTxData[0] = (ACC_FIFO_DATA_ADDR | BMI088_SPI_READ_CODE);
    HAL_SPI_TransmitReceive(&BMI088_SPI, pTxData, pRxData, 3, 1000); // 发送地址 返回空字节 返回帧头

    head = pRxData[2] & 0xfb; // 读取帧头高6位
    switch (head)
    {
    case (0x84): // 1数据帧
        uint8_t buff1[6];
        HAL_SPI_Receive(&BMI088_SPI, buff1, 6, 1000); // 读取6个字节的数据
        while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_RX)
            ;
        break;
    case (0x40): // 2跳过帧
        // 读取跳过帧的字节数
        uint8_t garbager[1] = {0};
        HAL_SPI_Receive(&BMI088_SPI,garbager, 1, 1000);

        uint8_t buff2[1024] = {0};
        HAL_SPI_Receive(&BMI088_SPI, buff2, 1024, 1000);
        while (HAL_SPI_GetState(&BMI088_SPI) == HAL_SPI_STATE_BUSY_RX);
        for(int i=0;i<1024;i++)
        {
            xQueueSend(bmi088_queue, &buff2[i],0);
        }
        break;

    case (0b01000100): // 3时间帧
        uint8_t buff3[3]={0};
        HAL_SPI_Receive(&BMI088_SPI, buff3, 3, 1000);
        data->sensor_time = buff3[0] * SENSORTIME_0_UNIT + buff3[1] * SENSORTIME_1_UNIT + buff3[2] * SENSORTIME_2_UNIT;
        break;

    case (0b01001000): // 4配置帧
        uint8_t buff4[1] = {0};
        HAL_SPI_Receive(&BMI088_SPI, buff4, 1, 1000);
        break;

    case (0b01010000): // 5丢弃帧
        uint8_t buff5[1] = {0};
        HAL_SPI_Receive(&BMI088_SPI, buff5, 1, 1000);
        break;

    default:

        break;
    }

    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
}